#!/usr/bin/python3

import rospy
import math
import tf
import tf2_ros
from geometry_msgs.msg import Pose, Twist, TransformStamped, PointStamped
import time
import numpy as np
from std_msgs.msg import Float64MultiArray, Header
from sensor_msgs.msg import LaserScan, JointState
from gazebo_msgs.msg import ModelStates
import copy
from tf2_geometry_msgs import tf2_geometry_msgs
import sys
from pogo_group.srv import CalibratePogoGroup, CalibratePogoGroupResponse
import yaml
from pogo_device import pogo_device
from digital_twin_interface import pogo_group_service

class pogo_group_control(pogo_group_service):
    def __init__(self,nodename, saved_calibrate_file, ns_prefix):
        super(pogo_group_control,self).__init__(nodename,saved_calibrate_file,ns_prefix)
        #rospy.Subscriber("/gazebo/model_states", ModelStates, self.cb_modelStates)
        #debug_prefix = "/pogo_group/"
        debug_prefix = ""

        self.pogo_group_num = rospy.get_param(debug_prefix + "pogo_group_config/single_pogo_num")
        self.pogo_axis_index = rospy.get_param(debug_prefix + "pogo_group_config/hardware_interface")

        # self._device = pogo_device('192.168.20.128.1.1','192.168.20.128',hardware_interface)

        self.js = JointState()
        self.js.header = Header()
        self.js.name = []
        self.js.position = []
        self.js.velocity = []
        self.js.effort = []

        self.sub = rospy.Subscriber("/beckhoff_controller/named_status", JointState, self.beckhoff_status_cb)
        self.pub = rospy.Publisher(debug_prefix + 'pogo_group_joint_states', JointState, queue_size=1)

    def beckhoff_status_cb(self,msg):
        status_joint = {msg.name[i]:msg.position[i] for i in range(len(msg.name))}

        joint_state_position = []
        joint_state_name = [] 
            
        for i in range(len(self.pogo_axis_index)):
            for j in range(len(self.pogo_axis_index[i])):
                joint_state_name.append("pogo"+str(i)+"_joint"+str(j+1))
                single_pogo_index = ".g_sAxisArr["+str(self.pogo_axis_index[i][j])+"].NcToPlc.ActPos"
                joint_state_position.append(status_joint[single_pogo_index]/1000.0)

        self.js.name = joint_state_name
        self.js.position = joint_state_position
        self.js.header.stamp = rospy.Time.now()
        self.pub.publish(self.js)

    def run(self):
        r = rospy.Rate(5)

        while (not rospy.is_shutdown()):
            # hardware_joints = self._device.readLegControlLength()

            # joint_state_positions = []
            # joint_state_name = [] 

            # for i in range(len(hardware_joints)):
            #     single_pogo_joint_names = []
            #     for j in range(len(hardware_joints[i])):
            #         single_pogo_joint_names.append("pogo"+str(i)+"_joint"+str(j+1))
            #         joint_state_positions.append(hardware_joints[i][j])
            #     joint_state_name.extend(single_pogo_joint_names)

            # self.js.name = joint_state_name
            # self.js.position = joint_state_positions
            # self.js.header.stamp = rospy.Time.now()
            # self.pub.publish(self.js)

            r.sleep()

if __name__ == '__main__':
    prefix = "pogo_group"
    calibration_file = "/home/zhangjiwen/catkin_dtsystem/src/aero_compiling_common/pogo_group/config/sample.yaml"

    if(len(sys.argv)>1):
        prefix = sys.argv[1]
        print("file written to prefix",prefix)

    if(len(sys.argv)>2):
        calibration_file = sys.argv[2]
        print("file written to ",calibration_file)

    server = pogo_group_control('pogo_group_service',calibration_file,prefix)
    # server.run()
    rospy.spin()
